/*****************************************************************************
* File Name: mpu6500.c
*
* Version: 1.0
*
* Description:
* This file contains API for motion sensor
*
* Note:
* Define DISABLE_MOTION_SENSOR in platform.h to disable motion sensor module
*
* Owner: KMVP
*
* Related Document:
* SCB component datasheet
*
* Hardware Dependency:
* MPU6500 Datasheet
*
* Code Tested With:
* 1. PSoC Creator 3.1 
* 2. ARM GCC 4.8.4
******************************************************************************
* Copyright (2015), Cypress Semiconductor Corporation.
******************************************************************************
* This software is owned by Cypress Semiconductor Corporation (Cypress) and is
* protected by and subject to worldwide patent protection (United States and
* foreign), United States copyright laws and international treaty provisions.
* Cypress hereby grants to licensee a personal, non-exclusive, non-transferable
* license to copy, use, modify, create derivative works of, and compile the
* Cypress Source Code and derivative works for the sole purpose of creating
* custom software in support of licensee product to be used only in conjunction
* with a Cypress integrated circuit as specified in the applicable agreement.
* Any reproduction, modification, translation, compilation, or representation of
* this software except as specified above is prohibited without the express
* written permission of Cypress.
*
* Disclaimer: CYPRESS MAKES NO WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, WITH
* REGARD TO THIS MATERIAL, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. 
* Cypress reserves the right to make changes without further notice to the
* materials described herein. Cypress does not assume any liability arising out
* of the application or use of any product or circuit described herein. Cypress
* does not authorize its products for use as critical components in life-support
* systems where a malfunction or failure may reasonably be expected to result in
* significant injury to the user. The inclusion of Cypress' product in a life-
* support systems application implies that the manufacturer assumes all risk of
* such use and in doing so indemnifies Cypress against all charges. Use may be
* limited by and subject to the applicable Cypress software license agreement.
*****************************************************************************/
#include "motion_sensor_hal.h"

#ifndef DISABLE_MOTION_SENSOR

/*****************************************************************************
* Function Name: MPU6500_GYRO_SetPowerDownMode()
******************************************************************************
* Summary:
* This function is used to put MPU6500 gyroscope only in power down 
*
* Parameters:
* None
*
* Return:
* None
*
* Theory:
*
* Side Effects:
* None
*
* Note:
* None
*****************************************************************************/
void MPU6500_GYRO_SetPowerDownMode(void)
{   
    uint8 regData;

    /* Switch on internal PLL as GYRO will be OFF */
    /* Set CYCLE = 1 and SLEEP = 0 and TEMP_DIS = 1 and STBY_*G = 1 */
    regData = (MPU6500_PWR_MGMT_1_CYCLE | MPU6500_PWR_MGMT_1_INTERNALOSC | MPU6500_PWR_MGMT_1_TEMP_DIS);
    MPU6500_WriteRegister(MPU6500_PWR_MGMT_1_ADR, regData);

    regData = MPU6500_PWR_MGMT_2_LP_WAKE_CTRL | MPU6500_PWR_MGMT_2_STBY_ZG | \
                                    MPU6500_PWR_MGMT_2_STBY_YG | MPU6500_PWR_MGMT_2_STBY_XG;
    MPU6500_WriteRegister(MPU6500_PWR_MGMT_2_ADR, regData);
}

/*****************************************************************************
* Function Name: MPU6500_ACC_SetPowerDownMode()
******************************************************************************
* Summary:
* This function is used to put MPU6500 ACC in Power down (Gyroscope is always already 
* powered down when this function is called).
*
* Parameters:
* None
*
* Return:
* None
*
* Theory:
* None
*
* Side Effects:
* None
*
* Note:
* None
*****************************************************************************/
void MPU6500_ACC_SetPowerDownMode(void)
{
  uint8 regData;
  
  /* At this step, GYRO is already OFF so ready to switch off everybody with SLEEP mode
   * Need to stay necessarily on internal PLL (we are already on it since GYRO is already OFF)
   * Currently it is SLEEP bit which is used but deactivation of each axis of ACC (like done for GYRO) could also be done
   */
    regData = (MPU6500_PWR_MGMT_1_CYCLE | MPU6500_PWR_MGMT_1_INTERNALOSC | MPU6500_PWR_MGMT_1_TEMP_DIS);
    MPU6500_WriteRegister(MPU6500_PWR_MGMT_1_ADR, regData);
}

/*****************************************************************************
* Function Name: MPU6500_Stop()
******************************************************************************
* Summary:
* This function is used to put MPU6500 ACC and GYRO in Power down 
*
* Parameters:
* None
*
* Return:
* None
*
* Theory:
*
* Side Effects:
* None
*
* Note:
* None
*****************************************************************************/
void MPU6500_Stop(void)
{
  MPU6500_GYRO_SetPowerDownMode();
  MPU6500_ACC_SetPowerDownMode();
}

/*****************************************************************************
* Function Name: MPU6500_Read_WhoAmI()
******************************************************************************
* Summary:
* This function is used to read the  WHOAMI Register
*
* Parameters:
* None
*
* Return:
* None
*
* Theory:
* None
*
* Side Effects:
* None
*
* Note:
* It has no practical use except to check if I2C communication is working or not
* It is resposible of the Caller to send the valid pointer to this API
*****************************************************************************/
void MPU6500_Read_WhoAmI(uint8 * data) 
{
    *data = MPU6500_ReadRegister(MPU6500_WHOAMI_ADR);
}

/*****************************************************************************
* Function Name: MPU6500_GYRO_GetSample()
******************************************************************************
* Summary:
* This function is used to Get X, Y and Z GYRO informations
*
* Parameters:
* int16 *bufferData
*
* Return:
* None
*
* Theory:
* None
*
* Side Effects:
* None
*
* Note:
* It is resposible of the Caller to send the valid pointer to this API
*****************************************************************************/
void MPU6500_GYRO_GetSample(int16_t * bufferData)
{
    uint8 tempbuf[MPU6500_GYROSCOPE_SAMPLES];
    MPU6500_ReadMultiRegister(MPU6500_GYRO_OUT_X_H_ADR, tempbuf, MPU6500_GYROSCOPE_SAMPLES);
    bufferData[0]= (tempbuf[0] << BIT_8_POSITION) + tempbuf[1];
    bufferData[1]= (tempbuf[2] << BIT_8_POSITION) + tempbuf[3];
    bufferData[2]= (tempbuf[4] << BIT_8_POSITION) + tempbuf[5];
}

/*****************************************************************************
* Function Name: MPU6500_ACC_GetSample()
******************************************************************************
* Summary:
* This function is used to Read accelerometer samples
*
* Parameters:
* int16 *lAccValue - A table of 3 byte for assigning accelerometer sample
*
* Return:
* None
*
* Theory:
* None
*
* Side Effects:
* None
*
* Note:
* It is resposible of the Caller to send the valid pointer to this API
*****************************************************************************/
void MPU6500_ACC_GetSample(int16_t * lAccValue)
{
    uint8 tempbuf[MPU6500_ACCELEROMETER_SAMPLES];

    MPU6500_ReadMultiRegister(MPU6500_ACCEL_XOUT_H_ADR, tempbuf, MPU6500_ACCELEROMETER_SAMPLES);
    lAccValue[0] = (int16_t)((tempbuf[0] << BIT_8_POSITION) + tempbuf[1]);
    lAccValue[1] = (int16_t)((tempbuf[2] << BIT_8_POSITION) + tempbuf[3]);
    lAccValue[2] = (int16_t)((tempbuf[4] << BIT_8_POSITION) + tempbuf[5]);
}

/*****************************************************************************
* Function Name: MPU6500_StopMotionDetection()
******************************************************************************
* Summary:
* This function is used to disable motion detection mode
*
* Parameters:
* None
*
* Return:
* None
*
* Theory:
* None
*
* Side Effects:
* None
*
* Note:
* None
*****************************************************************************/
void MPU6500_StopMotionDetection(void)
{
    uint8 data;

    /* Disable Cycle Mode (Low Power Mode) */
    data = ( MPU6500_PWR_MGMT_1_INTERNALOSC | MPU6500_PWR_MGMT_1_TEMP_DIS );
    MPU6500_WriteRegister(MPU6500_PWR_MGMT_1_ADR, data);

    /* disable MOTION DETECTION IP */
    data = 0;
    MPU6500_WriteRegister(MPU6500_INT_ENABLE_ADR, data);

    /* clear INT */
    MPU6500_ReadRegister(MPU6500_INT_STATUS_ADR);
}

/*****************************************************************************
* Function Name: MPU6500_StartMotionDetection()
******************************************************************************
* Summary:
* This function is puth the Motion sensor in low power Motion Detect mode
* with INT pin configured as level interrupt 
*
* Parameters:
* None
*
* Return:
* None
*
* Theory:
*
* Side Effects:
* None
*
* Note:
* None
*****************************************************************************/
void MPU6500_StartMotionDetection(void)
{
    uint8 data;
    /* Set accelerometer LPF setting to 256Hz bandwidth */
    data = MPU6500_CONFIG_LPFILTER_256Hz;
    MPU6500_WriteRegister(MPU6500_CONFIG_ADR, data);

    /* enable motion interrupt */
    data = MPU6500_INT_ENABLE_MOT_EN;
    MPU6500_WriteRegister(MPU6500_INT_ENABLE_ADR, data);

    /* set Motion Detection Threshold */
    data = MOTION_WAKEUP_THRESHOLD; /* unit is mg/LSB (gravity is removed by motion select logic)*/
    MPU6500_WriteRegister(MPU6500_MOT_THR_ADR, data);

    /* Set accelerometer full scale range settings */
    data = (MPU6500_ACCEL_CONFIG_AFSEL_FS2G );
    MPU6500_WriteRegister(MPU6500_ACCEL_CONFIG_ADR, data);

    /* Set the frequency of wake-up and to 20Hz and disable gyro */
    data = MPU6500_PWR_MGMT_2_LP_WAKE_CTRL | MPU6500_PWR_MGMT_2_STBY_ZG | \
                            MPU6500_PWR_MGMT_2_STBY_YG | MPU6500_PWR_MGMT_2_STBY_XG;
    MPU6500_WriteRegister(MPU6500_PWR_MGMT_2_ADR, data);

    /* configuration interrupt */
    data = (MPU6500_INT_PIN_CFG_LATCH_INT_EN | MPU6500_INT_PIN_CFG_I2C_BYPASS_EN);
    MPU6500_WriteRegister(MPU6500_INT_PIN_CFG_ADR, data);

    /* clear interrupt flag */
    MPU6500_WriteRegister(MPU6500_INT_STATUS_ADR, data);

    /* set output data rate value (ODR value) */
    data = 5;
    MPU6500_WriteRegister(MPU6500_LP_ACCEL_ODR_ADR, data);

    /* Enable Cycle Mode (Low Power Mode)*/
    data = ( MPU6500_PWR_MGMT_1_INTERNALOSC | MPU6500_PWR_MGMT_1_CYCLE | MPU6500_PWR_MGMT_1_TEMP_DIS );
    MPU6500_WriteRegister(MPU6500_PWR_MGMT_1_ADR, data);
}

/*****************************************************************************
* Function Name: MPU6500_GYROACC_SetNormalMode()
******************************************************************************
* Summary:
* This function is used to put MPU6500 GYRO in Normal operating mode.
*
* Parameters:
* uint8 FS_SEL - full-scale range
* uint8 DLPF_CFG - digital low pass filter configuration 
* uint8 HPF - digital high pass filter configuration 
* uint8 FS - accelerometer full scale range
*
* Return:
* None
*
* Theory:
* None
*
* Side Effects:
* None
*
* Note:
* None
*****************************************************************************/
static void MPU6500_GYROACC_SetNormalMode(uint8 FS_SEL, uint8 DLPF_CFG, uint8 HPF, uint8 FS)
{
    uint8 regData;
    /* osc: 8kHz -> 500, osc: 1kHz -> 333, 200, 100, 40, 20, 10
     *  div = (osc / Fs) - 1  
     */
    uint8 sampleDiv[7] = {15, 2, 4, 9, 24, 49, 99};

    /* Re-enable GYRO since GYRO can be OFF at that step if go from State MOTION_DETECT to State POINTING */
    regData = 0;
    MPU6500_WriteRegister(MPU6500_PWR_MGMT_2_ADR, regData);

    /* PLL with Z axis gyroscope reference */
    regData = (MPU6500_PWR_MGMT_1_PLL_ZGYRREF | MPU6500_PWR_MGMT_1_TEMP_DIS);
    MPU6500_WriteRegister(MPU6500_PWR_MGMT_1_ADR, regData);
    
    /* DLPF_CFG : Digital Low-Pass Filter for ACC and GYRO
       Config 94Hz, Fs=1kHz for ACC
       Config 98Hz so GYRO Output Rate=1kHz for GYRO */
    regData = DLPF_CFG;
    MPU6500_WriteRegister(MPU6500_CONFIG_ADR, regData);

    /* Divider from the GYRO Output Rate used to generate the Sample Rate
     * Sample Rate = GYRO Output Rate/(1+param)
     * Sample Rate Divider to obtain is 200 Hz, 1 kHz / (4 + 1) = 200 Hz 
     */
    MPU6500_WriteRegister(MPU6500_SMPLRT_DIV_ADR, sampleDiv[DLPF_CFG]);

    /* FS_SEL Gyro 2000 °/s */
    regData= FS_SEL;
    MPU6500_WriteRegister(MPU6500_GYRO_CONFIG_ADR, regData);

    /* Accel 16G
     * DHPF : motion detection High pass filter 0,63Hz to eliminate bias due to gravity 
     */
    regData = (HPF | FS);
    MPU6500_WriteRegister(MPU6500_ACCEL_CONFIG_ADR, regData);

    /* Enable accel and gyro */
    regData = 0;
    MPU6500_WriteRegister(MPU6500_PWR_MGMT_2_ADR, regData);
}

/*****************************************************************************
* Function Name: MPU6500_Start()
******************************************************************************
* Summary:
* This function is used to start gyroscope, accelerometer data sampling with
* a specific configuration.
*
* Parameters:
* None
*
* Return:
* None
*
* Theory:
* None
*
* Side Effects:
* None
*
* Note:
* None
*****************************************************************************/
void MPU6500_Start(void)
{
    MPU6500_GYROACC_SetNormalMode(MPU6500_GYRO_CONFIG_FULLSCALE_2000, \
                                            MPU6500_CONFIG_LPFILTER_98Hz, \
                                            MPU6500_ACCEL_CONFIG_HPF_0_63HZ, \
                                            MPU6500_ACCEL_CONFIG_AFSEL_FS2G);
}

/*****************************************************************************
* Function Name: MPU6500_WriteRegister()
******************************************************************************
* Summary:
* This function writes the MPU6500 register using I2C component
*
* Parameters:
* uint8 address - register address inside the sensor chip to be written
* uint8 data - 8 bit data to be written
*
* Return:
* None
*
* Theory:
* First the IC Slave is written with the value of address and transaction is ended. 
* This sets the address pointer of the sensor chip to the passed address.
* Then in a new I2C transaction the value to be written to that address is written as a slave write
*
* Side Effects:
* None
* 
* Note:
* None
*****************************************************************************/
void MPU6500_WriteRegister(uint8 address, uint8 data)
{
    uint8  buffer[2];   
    /* Initialize buffer with packet */
    buffer[0] = address;
    buffer[1] = data;
    (void) Motion_Sensor_I2C_I2CMasterWriteBuf(MPU6500_I2C_SLAVE_ADDRESS, buffer, sizeof(buffer), Motion_Sensor_I2C_I2C_MODE_COMPLETE_XFER);

    while (0u == (Motion_Sensor_I2C_I2CMasterStatus() & Motion_Sensor_I2C_I2C_MSTAT_WR_CMPLT))
    {
        /* Waits until master completes write transfer */
            CySysPmSleep(); /* save power while waiting*/
    }
    (void) Motion_Sensor_I2C_I2CMasterClearStatus(); 

}

/*****************************************************************************
* Function Name: MPU6500_ReadRegister()
******************************************************************************
* Summary:
* This function reads the MPU6500 register using I2C component
*
* Parameters:
* uint8 address - register address inside the sensor chip to be written
* 
*
* Return:
* uint8 - data read from the sensor
*
* Theory:
* First the IC Slave is written with the value of address and transaction is ended. 
* This sets the address pointer of the sensor chip to the passed address.
* Then in a new I2C transaction read operation is carried out
*
* Side Effects:
* None
* 
* Note:
* None
*****************************************************************************/
uint8 MPU6500_ReadRegister(uint8 address)
{
    uint8  i2cData;

    /*write the address to be read first*/
    (void) Motion_Sensor_I2C_I2CMasterWriteBuf(MPU6500_I2C_SLAVE_ADDRESS, &address, sizeof(address), Motion_Sensor_I2C_I2C_MODE_COMPLETE_XFER);

    while (0u == (Motion_Sensor_I2C_I2CMasterStatus() & Motion_Sensor_I2C_I2C_MSTAT_WR_CMPLT))
    {
        /* Waits until master completes write transfer */
        CySysPmSleep(); /* save power while waiting*/
    }
    (void) Motion_Sensor_I2C_I2CMasterClearStatus();
    
    /* read operation*/    
    (void) Motion_Sensor_I2C_I2CMasterReadBuf(MPU6500_I2C_SLAVE_ADDRESS, &i2cData, sizeof(i2cData), Motion_Sensor_I2C_I2C_MODE_COMPLETE_XFER);

    while (0u == (Motion_Sensor_I2C_I2CMasterStatus() & Motion_Sensor_I2C_I2C_MSTAT_RD_CMPLT))
    {
        /* Waits until master complete read transfer */
       CySysPmSleep(); /* save power while waiting*/
    }

    (void) Motion_Sensor_I2C_I2CMasterClearStatus();
    return i2cData;
}

/*****************************************************************************
* Function Name: MPU6500_ReadMultiRegister()
******************************************************************************
* Summary:
* This function reads MPU6500 register with consecutive address using I2C component 
*
* Parameters:
* uint8 address - register address inside the sensor chip to be written
* uint8 *data   - pointer of the buffer to where read data should be stored
* uint8 len     - number of registers to be read
*
* Return:
* None
*
* Theory:
* First the IC Slave is written with the value of address and transaction is ended. 
* This sets the address pointer of the sensor chip to the passed address.
* Then in a new I2C transaction read operation is carried out
*
* Side Effects:
* None
* 
* Note:
* None
*****************************************************************************/
void MPU6500_ReadMultiRegister(uint8 address, uint8 *data, uint8 len)
{
    (void) Motion_Sensor_I2C_I2CMasterWriteBuf(MPU6500_I2C_SLAVE_ADDRESS, &address, sizeof(address), Motion_Sensor_I2C_I2C_MODE_COMPLETE_XFER);

    while (0u == (Motion_Sensor_I2C_I2CMasterStatus() & Motion_Sensor_I2C_I2C_MSTAT_WR_CMPLT))
    {
        /* Waits until master completes write transfer */
        CySysPmSleep(); /* save power while waiting*/
    }
    (void) Motion_Sensor_I2C_I2CMasterClearStatus();     

    /* read operation*/
       (void) Motion_Sensor_I2C_I2CMasterReadBuf(MPU6500_I2C_SLAVE_ADDRESS, data, len, Motion_Sensor_I2C_I2C_MODE_COMPLETE_XFER);

    while (0u == (Motion_Sensor_I2C_I2CMasterStatus() & Motion_Sensor_I2C_I2C_MSTAT_RD_CMPLT))
    {
        /* Waits until master complete read transfer */
        CySysPmSleep(); /* save power while waiting*/
    }

    (void) Motion_Sensor_I2C_I2CMasterClearStatus();
}

#endif /* DISABLE_MOTION_SENSOR */
